#include "MotorWrapper.h"

MotorWrapper::MotorWrapper(UINT32 motorChannel, MotorMode mode, bool flip) {
	//_motor = new CANJaguar(motorChannel);
	if (flip)
		_reverse = true;
	else
		_reverse = false;
	printf("Intializing channel %d\n", motorChannel);
	_motor = new CANJaguar(motorChannel);
	if (mode == speed) {
		_motor->ChangeControlMode(CANJaguar::kSpeed);
		_motor->SetPID(SPEED_PID_P,SPEED_PID_I,SPEED_PID_D);
		_motor->SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
		_motor->ConfigEncoderCodesPerRev(250);
		printf("Error: %s\n", _motor->GetError().GetMessage());
	} else if (mode == position) {
		_motor->ChangeControlMode(CANJaguar::kPosition);
		_motor->SetPID(POS_PID_P,POS_PID_I,POS_PID_D);
		_motor->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
		_motor->ConfigEncoderCodesPerRev(250);
		printf("Error: %s\n", _motor->GetError().GetMessage());
	} else if (mode == voltage) {
		_motor->ChangeControlMode(CANJaguar::kPercentVbus);
	}
	_motor->EnableControl();
	_mode = mode;
}

void MotorWrapper::SetEncoderCPP(int cpp) {
	_motor->ConfigEncoderCodesPerRev(cpp);
}

MotorWrapper::~MotorWrapper() {
	delete _motor;
}

void MotorWrapper::Enable() {
	_motor->EnableControl();
}

void MotorWrapper::Disable() {
	_motor->DisableControl();
}

float MotorWrapper::GetP() {
	return _motor->GetP();
}

float MotorWrapper::GetI() {
	return _motor->GetI();
}

float MotorWrapper::GetD() {
	return _motor->GetD();
}

float MotorWrapper::GetState() {
	if (_mode == speed)
		return _motor->GetSpeed();
	else if (_mode == position)
		return _motor->GetPosition();
	return 0;
}

float MotorWrapper::GetSetpoint() {
	return _motor->Get();
}

void MotorWrapper::SetSetpoint(float setpoint) {
	if (_reverse)
		_motor->Set(-setpoint);
	else
		_motor->Set(setpoint);
}

void MotorWrapper::SetPID(float p, float i, float d) {
	_motor->SetPID(p,i,d);
}

void MotorWrapper::PIDWrite(float output) {
	if (_reverse)
		_motor->Set(-output);
	else
		_motor->Set(output);
}

float MotorWrapper::GetTemp() {
	return _motor->GetTemperature();
}

float MotorWrapper::GetVoltage() {
	return _motor->GetOutputVoltage();
}
